00100		TITLE SERVO
00200		.INSERT ASMBL.FAI[CMS,LCS]
00300	
00400	;Check if inverted encoder?
00500	;Put HSTTMR in shared ram for host set?
00600	;Flush STATUS?
00700	;If HSTTMR or CKWORD or ? then stop?
00800	;Check GPOSER and in servo formula for sign reverse?
00900	
01000	;I/O address definitions.
01100	   DAC ← 100000	;8 bit DAC.
01200	   JCR ← 120000	;Joint control output register.
01300	   ENCL ← 140000	;Encoder mux low.
01400	   ENCH ← 140001	;Encoder mux high.
01500	
01600	   STKSIZ ← 377	;Stack size.
01700	   LSBENB ← 40	;Enable LSB servo.
01800	
01900	;Zero page variables.
02000	;Not shared.
02100	
02200	DSPAT:	BLOCK 2	;Dispatch address for commands.
02300	DEFCMD:	0	;Deferred command.
02400	SAVPOS:	BLOCK 2	;Position for deferred servo command.
02500	
02600	CMDVEL:	BLOCK 2	;Commanded velocity.
02700	CURVEL:	BLOCK 2	;Current velocity.
02800		0	;SETPT-1.
02900	SETPT:	BLOCK 2	;Current setpoint.
03000		0	;SETINC-1.
03100	SETINC:	BLOCK 2	;Interpolating increment for setpoints.
03200	OLDSP:	BLOCK 2	;Last commanded setpoint, for CMDVEL.
03300	POSERR:	BLOCK 2	;Current position error.
03400	DACSIG:	BLOCK 2	;Scratch.
03500	
03600	INCTR:	0	;Count the interpolations.
03700	HSTTMR:	0	;Count ticks between host commands.
03800	
03900	LOGTMP:	BLOCK 4	;Temp for the arithmetic routines.
04000	
04100	ZAPEND ← .-1	;Clear all the above in startup.
04200	
04300	TL:	0	;Scratch for grey to binary.
04400	TH:	0
04500	
04600	FTMP:	BLOCK 2	;Copy of friction coefficient for multiply.
     

00100	;Shared ram.
00200	   LOC 200	;Second half of zero page.
00300	
00400		0	;Locked.
00500	STATUS:	0	;Flags for the host.
00600	
00700	;MODE byte bits.
00800	;Bit	7	6	5	4	3	2	1	0
00900	;    servo   integ     lsb
01000	;     enlb    enlb    enbl
01100	
01200		0	;Locked.
01300	MODE:	0	;Mode bits from host.
01400	
01500	CKWORD:	BLOCK 2	;Host I/O check/command word.
01600	CMDPOS:	BLOCK 2	;Commanded position from host.
01700	
01800	;IOCTRL byte bits.
01900	;Bit	7	6	5	4	3	2	1	0
02000	;      in		      lsb   integ     pos
02100	;     tol		     enlb  disabl    mode
02200	
02300		0	;Locked.
02400	IOCTRL:	0	;Copy of JCR output port.
02500	
02600	CURPOS:	BLOCK 2	;Current position.
02700	
02800	;NINTER = function of INTSCL?
02900		0	;Locked.
03000	NINTER:	0	;# of interpolations between position
03100			;commands.
03200		0	;Locked.
03300	INTSCL:	0	;# of bits to shift setpoint dif for
03400			;interpolating.
03500		0	;Locked.
03600	HSTLIM:	0	;# of clock ticks allowed between host
03700			;commands.
03800	FRICTN:	BLOCK 2	;Viscous damping coefficient.
03900	GRAVTY:	BLOCK 2	;DC offset for gravity.
04000	POSTOL:	BLOCK 2	;Half-width of position tolerance band.
04100	INTTOL:	BLOCK 2	;Half-width of integration band.
04200	
04300	
04400	;Start of prom.
04500	   LOC 174000
04600	
04700	INITBL:	STATUS	↔	200
04800		MODE	↔	0
04900	
05000		NINTER	↔	=32
05100		INTSCL	↔	5
05200	
05300		HSTLIM	↔	=48
05400	
05500		377	;End of INITBL flag.
     

00100	;Power on and restart reset.
00200	STOP:	SEI	;Disable interrupts and stop.
00300	
00400	START:	CLD
00500		LDXI	STKSIZ	;Setup stack.
00600		TXS
00700	
00800		LDAI	0
00900		LDXI	ZAPEND
01000	RLOOP:	STAZX	0	;Reset ram.
01100		DEX
01200		BPL	RLOOP
01300		STA	DAC	;Clear DAC.
01400	
01500		LDXI	370	;-8.
01600	ZSR:	STAZX	FRICTN+10	;Clear shared ram.
01700		INX
01800		BMI	ZSR
01900	
02000		TAY
02100		BEQ	RSTDEF	;Jump
02200	
02300	
02400	DLOOP:	INY
02500		LDAY	INITBL	;Init ram.
02600		STAZX	0
02700		INY
02800	
02900	RSTDEF:	LDXY	INITBL
03000		CPXI	377
03100		BNE	DLOOP
03200	
03300		JSR	GETPOS	;Read encoder and convert to binary.
03400	
03500	;Sets the current position to the converted encoder value, the
03600	;setpoint the same, clears the setpoint interpolating increment,
03700	;and goes into stop mode.
03800		STAZ	CURPOS	;Set the current position.
03900		STXZ	CURPOS+1;Unlock.
04000	
04100		STAZ	SETPT	;Set the setpoint.
04200		STXZ	SETPT+1
04300		STAZ	OLDSP
04400		STXZ	OLDSP+1
04500	
04600		LDAI	75	;I/O control bits for servo enable on,
04700		STAZ	IOCTRL	;all others off.
04800		STA	JCR
04900	
05000		LDAI	0
05100		STAZ	SETPT-1	;Clear the setpoint extension,
05200		STAZ	SETINC-1;the interpolator,
05300		STAZ	SETINC
05400		STAZ	SETINC+1
05500		STAZ	CMDVEL	;and the commanded velocity.
05600		STAZ	CMDVEL+1
05700	
05800		STAZ	DEFCMD	;Clear the deferred command flag.
05900	
06000		CLI	;End of reset.
     

00100	RSTCKW:	LDAI	0	;Reset check word.
00200		SEI
00300		STAZ	CKWORD	;Lock.
00400		STAZ	CKWORD+1;Unlock.
00500		CLI
00600	;Idle loop. Wait for command.
00700	IDLE:	LDAZ	CKWORD+1;Check for new check word.
00800		BEQ	IDLE	;Not equal if bit 7 is 2's comp. of low byte.
00900	
01000		CLC
01100		SEI
01200		ADCZ	CKWORD	;Lock.
01300		LDXZ	CKWORD+1;Unlock.
01400		CLI
01500		TAY
01600		BNE	STOP	;Check word error.
01700	   ;Check here for immediate or deferred.
01800		LDAZ	DEFCMD	;Check if no TICK?
01900		BNE	STOP
02000	
02100		SEI
02200		LDAZ	CMDPOS	;Read position for servo command.
02300		LDYZ	CMDPOS+1;Unlock.
02400		CLI
02500	
02600		STAZ	SAVPOS	;Save it for later.
02700		STYZ	SAVPOS+1
02800	
02900		STXZ	DEFCMD	;Save deferred command pointer.
03000	
03100		JMP	RSTCKW	;Handshake with host via CKWORD.
     

00100	;Clock tick interrupt.
00200	TICK:	PHA	;Save state.
00300		TXA
00400		PHA
00500		TYA
00600		PHA
00700	
00800		JSR	GETPOS	;Read position and convert to binary.
00900	
01000		SEC
01100		SBCZ	CURPOS	;Subtract the old position
01200		STAZ	CURVEL	;yielding the velocity.
01300		TXA	;High byte of binary position.
01400		SBCZ	CURPOS+1;Unlock.
01500		STAZ	CURVEL
01600	
01700		STYZ	CURPOS	;Update the current position.
01800		STXZ	CURPOS+1;Unlock.
01900		DECZ	HSTTMR	;Count the ticks since the last command
02000		BPL	HOSTOK	;and check for timeout.
02100	
02200		LDAI	0	;Host dead. Stop.
02300		STAZ	HSTTMR
02400		STAZ	CMDVEL
02500		STAZ	CMDVEL+1
02600	
02700	HOSTOK:	LDAI	4
02800		BITZ	IOCTRL	;If position mode is off,
02900		BNE	INTRS
03000		JMP	CURSRV	;don't servo.
03100	
03200	;Interpolate the setpoints.
03300	INTRS:	CLC
03400		LDAZ	SETPT-1
03500		ADCZ	SETINC-1;Add the increment to the setpoint.
03600		STAZ	SETPT-1
03700		LDAZ	SETPT
03800		ADCZ	SETINC
03900		STAZ	SETPT
04000		LDAZ	SETPT+1
04100		ADCZ	SETINC+1
04200		STAZ	SETPT+1
04300	
04400		DECZ	INCTR	;Check if this is the last interpolation.
04500		BNE	GPOSER
04600	
04700		LDAI	0	;Clear SETINC if done interpolating.
04800		STAZ	SETINC-1
04900		STAZ	SETINC
05000		STAZ	SETINC+1
05100	
05200	;Calculate the position error.
05300	GPOSER:	SEC
05400		LDAZ	CURPOS	;POSERR ← CURPOS - SETPT.
05500		SBCZ	SETPT
05600		STAZ	POSERR
05700		LDAZ	CURPOS+1
05800		SBCZ	SETPT+1
05900		STAZ	POSERR+1
     

00100		BITZ	MODE	;If servo is disabled, we're
00200		BPL	OOTOL	;automatically out of tolerance
00300	
00400		LDAZ	POSERR+1;Test the sign of pos error.
00500		BMI	NEGPER
00600	
00700		LDAZ	POSTOL	;Positive. Compare with tol.
00800		CMPZ	POSERR
00900		LDAZ	POSTOL+1;Unlock.
01000		SBCZ	POSERR+1
01100		BCS	TOLOK	;In tolerance.
01200		BCC	OOTOL	;Jump.
01300	
01400	NEGPER:	CLC	;Negative. Add the tolerance.
01500		LDAZ	POSTOL	;Lock.
01600		ADCZ	POSERR
01700		LDAZ	POSTOL+1;Unlock.
01800		ADCZ	POSERR+1
01900		BCS	TOLOK	;In tolerance.
02000	
02100	OOTOL:	LDAZ	IOCTRL	;Out of tolerance.
02200		ANDI	177	;Turn off the in tolerance
02300		BNE	WCNTRL	;indicator. Jump.
02400	
02500	TOLOK:	LDAZ	IOCTRL	;In tolerance. Turn it on.
02600		ORAI	200
02700	WCNTRL:	STAZ	IOCTRL
02800		STA	JCR	;Copy it to output.
02900	
03000		BITZ	MODE	;If intergration is disabled,
03100		BVC	OOBAND	;turn it off.
03200		LDAZ	POSERR+1;Test sign of position error.
03300		BMI	ADTOL
03400	
03500		LDAZ	INTTOL	;Positive. Compare with tol.
03600		CMPZ	POSERR
03700		LDAZ	INTTOL+1;Unlock.
03800		SBCZ	POSERR+1
03900		BCS	INBAND	;In band. Turn on integrator.
04000		BCC	OOBAND	;Jump.
04100	
04200	ADTOL:	CLC	;Negative. Add the tolerance.
04300		LDAZ	INTTOL	;Lock.
04400		ADCZ	POSERR
04500		LDAZ	INTTOL+1;Unlock.
04600		ADCZ	POSERR+1
04700		BCS	INBAND	;Check if in band.
04800	
04900	OOBAND:	LDAZ	IOCTRL	;Out of band. Turn off
05000		ORAI	10	;integration by setting the
05100		ANDI	357	;control bit. LSB servo off.
05200		BNE	WCTRL2	;Jump.
     

00100	INBAND:	LDAI	LSBENB	;In band. Is LSB servo enabled?
00200		BITZ	MODE
00300		BEQ	RCNTRL
00400	
00500		LDAZ	POSERR	;Yes. Is the error exactly 0?
00600		ORAZ	POSERR+1
00700		BNE	RCNTRL
00800	
00900		LDAZ	IOCTRL	;It is. Integration off, LSB
01000		ORAI	30	;servo on.
01100		BNE	WCTRL2	;Jump.
01200	
01300	RCNTRL:	LDAZ	IOCTRL	;LSB disabled or error
01400		ANDI	347	;not zero. LSB servo off,
01500				;integration on.
01600	
01700	WCTRL2:	STAZ	IOCTRL
01800		STA	JCR	;Output it.
01900	
02000		LDYZ	CURVEL	;Get the velocity,
02100		LDAZ	CURVEL+1
02200		JSR	LOG
02300	
02400		LDXZ	FRICTN	;(Copy friction for multiply.)
02500		STXZ	FTMP
02600		LDXZ	FRICTN+1;Unlock.
02700		STXZ	FTMP+1
02800		LDXI	FTMP	;multiply by the friction
02900		JSR	MULTIP	;coefficient,
03000		JSR	EXP
03100	
03200		TAX	;Save high byte.
03300		TYA	;Get low byte.
03400		CLC	;add the position error...
03500		ADCZ	POSERR
03600		STAZ	DACSIG
03700		TXA
03800		ADCZ	POSERR+1
03900		STAZ	DACSIG+1
04000	
04100		CLC	;...and the gravity offset.
04200		LDAZ	DACSIG
04300		ADCZ	GRAVTY	;Lock.
04400		TAY	;Save low byte.
04500		LDAZ	GRAVTY+1;Unlock.
04600		ADCZ	DACSIG+1
04700	
04800		JSR	PUTDAC	;Put result out to the DAC.
     

00100	CMDSP:	LDAZ	DEFCMD	;Check for a command.
00200		BEQ	INTXIT
00300	
00400		ANDI	2	;Low nibble command bit.
00500		TAX
00600		LDAX	CMDTBL	;Get command address.
00700		STAZ	DSPAT
00800		LDAX	CMDTBL+1
00900		STAZ	DSPAT+1
01000	
01100		JMPIN	DSPAT	;Execute command.
01200	
01300	CMDEND:	LDAI	0	;Done with deferred command.
01400		STAZ	DEFCMD	;Reset command word.
01500	
01600	INTXIT:	PLA	;Restore state and dismiss interrupt.
01700		TAY
01800		PLA
01900		TAX
02000		PLA
02100		RTI
02200	
02300	CURSRV:	LDAI	0	;Not servoing ("Current mode")...
02400		STAZ	SETPT-1	;Make the setpoint track
02500		LDAZ	CURPOS	;the current position in order to
02600		STAZ	SETPT	;keep the arm from twitching when
02700		LDAZ	CURPOS+1;the host enables the servo. Unlock.
02800		STAZ	SETPT+1
02900		JMP	CMDSP	;Go check on commands.
03000	
03100	CMDTBL:		;DEFERRED COMMAND TABLE.
03200		CMDEND∧377	;Nop.
03300		(CMDEND⊗-10)∧377
03400	
03500		CMDSRV∧377	;Servo command.
03600		(CMDSRV⊗-10)∧377
     

00100	;Deferred commands.
00200	CMDSRV:	LDAZ	MODE	;Servo command.
00300		ANDI	202	;Test for servo enabled.
00400		CMPI	200
00500		BEQ	ENBLD
00600		JMP	CMDEND	;No. End this command. 
00700	
00800	ENBLD:	LDAZ	SAVPOS	;Enabled.
00900		LDXZ	SAVPOS+1;Get commanded position.
01000		STAZ	DACSIG
01100		STXZ	DACSIG+1
01200		SEC
01300		SBCZ	SETPT	;Get differance between next position
01400		STAZ	SETINC	;and the last setpoint.
01500		TXA
01600		SBCZ	SETPT+1
01700		LDXI	0
01800		STXZ	SETPT-1	;Clear setpoint and increment extentions.
01900		STXZ	SETINC-1
02000		LDXZ	INTSCL
02100	
02200	SCAL:	CMPI	200	;Extend sign.
02300		RORA	;Divide the differance by the number of interpolations.
02400		RORZ	SETINC
02500		RORZ	SETINC-1
02600		DEX
02700		BNE	SCAL
02800	
02900		STAZ	SETINC+1;Which yields the interpolating increment.
03000		LDAZ	NINTER
03100		STAZ	INCTR	;Setup the interpolator count.
03200		SEC
03300		LDAZ	DACSIG
03400		SBCZ	OLDSP
03500		STAZ	CMDVEL	;CMDVEL ← CMDPOS - OLDSP.
03600		LDAZ	DACSIG+1
03700		SBCZ	OLDSP+1
03800		STAZ	CMDVEL+1
03900		LDAZ	DACSIG
04000		STAZ	OLDSP	;OLDSP ← CMDPOS.
04100		LDAZ	DACSIG+1
04200		STAZ	OLDSP+1
04300	
04400		LDAZ	IOCTRL
04500		ORAI	44	;Turn on servo and current mode enable bits.
04600		STAZ	IOCTRL
04700		STA	JCR	;Output it.
04800		LDAZ	HSTLIM	;Reset host timer.
04900		STAZ	HSTTMR
05000		JMP	CMDEND
     

00100	;Position conversion routine.
00200	GETPOS:	LDY	ENCL	;Read encoder.
00300		LDA	ENCH
00400		EORI	377	;Complement it.
00500	;Convert from grey to binary.
00600		STAZ	TH
00700		LSRA	;Shift by 1.
00800		EORZ	TH
00900		STAZ	TH
01000		TAX	;X ← high byte.
01100	
01200		TYA
01300		EORI	377	;Complement low byte.
01400		STAZ	TL
01500		RORA
01600		EORZ	TL
01700		STAZ	TL
01800	
01900		LSRZ	TH	;Shift by 2.
02000		RORA
02100		LSRZ	TH
02200		RORA
02300		EORZ	TL
02400		STAZ	TL
02500		TAY	;Y ← low byte.
02600	
02700		TXA	;Get high byte.
02800		EORZ	TH
02900		STAZ	TH
03000	
03100		LSRA	;Shift by 4.
03200		RORZ	TL
03300		LSRA
03400		RORZ	TL
03500		LSRA
03600		RORZ	TL
03700		LSRA
03800		RORZ	TL
03900	
04000		EORZ	TH
04100		STAZ	TH
04200		TYA
04300		EORZ	TL
04400		EORZ	TH	;Shift by 8.
04500		TAY	;Save low byte.
04600	
04700		LDXZ	TH	;Get high byte.
04800		BITZ	TH
04900		BVC	POS	;Check if negative.
05000		TXA
05100		ORAI	200	;Extend sign.
05200		TAX
05300	
05400	POS:	TYA	;Returns with position in A, Y (low) and X (high).
05500		RTS
     

00100	;DAC output subroutine.
00200	;Enter with 2 byte value in Y (low), A (high).
00300	;Clobbers all registers, but the 8 bits the DAC got are returned in A.
00400	PUTDAC:	BMI	NEGDAC	;Assuming the last inst. loaded A.
00500		CPYI	200	;Positive. Compare with 2↑7.
00600		SBCI	0
00700		BCC	INRNGE
00800	
00900	TOOHI:	LDYI	177	;Too high. Saturate positive.
01000		BNE	INRNGE	;Jump.
01100	
01200	NEGDAC:	CPYI	200	;Negative. Compare with -2↑7.
01300		SBCI	377
01400		BCS	INRNGE
01500	
01600	TOOLOW:	LDYI	200	;Too low. Saturate to -2↑7.
01700	
01800	INRNGE:	LDAY	VETBL	;Straighting it.
01900		STA	DAC	;Output 8 bits to the DAC.
02000		RTS
     

00100	;Arithmetic routines.
00200	;Enter with high byte in A, low in Y.
00300	;Returns A = characteristic and sign, Y = mantissa.
00400	;Clobbers X, LOGTMP, LOGTMP+1.
00500	LOG:	STYZ	LOGTMP	;Save the inputs.
00600		STAZ	LOGTMP+1
00700	
00800		LDXI	20+100	;Init characteristic to 15.
00900		CMPI	0	;Test sign of input.
01000		BPL	POSIN
01100		SEC	;Negative. 2's complement it.
01200		LDAI	0
01300		SBCZ	LOGTMP
01400		STAZ	LOGTMP
01500		LDAI	0
01600		SBCZ	LOGTMP+1
01700	POSIN:	BNE	NORML	;Is high byte zero?
01800		LDAZ	LOGTMP	;Yes. Low byte?
01900		BEQ	RTRN	;If so, return zero.
02000		LDYI	0	;Low nonzero. Shift left one
02100		STYZ	LOGTMP	;byte,
02200		LDXI	10+100	;change characteristic to 7.
02300	NORML:	DEX	;Normalize the number, counting the
02400		ASLZ	LOGTMP	;characteristic down. When the
02500		ROLA	;first "1" shifts out, we've subtracted
02600		BCC	NORML	;1 from the normalized number
02700		ASLZ	LOGTMP	;(This rounds the result)
02800		ADCI	=11	;and are left with the fraction
02900		TAY	;Adding 11 to that is equivalent to
03000		TXA	;adding 0.043.
03100		ADCI	0	;Propagate the carry into the
03200				;characteristic.
03300		ASLA	;Insert the sign bit from the saved
03400		ASLZ	LOGTMP+1;input.
03500		RORA
03600	RTRN:	RTS	;Done.
03700	
03800	;Enter with sign and characteristic in A, mantissa in Y
03900	;Returns 16-bit integer, low byte in Y, high in A.
04000	;Clobbers X, LOGTMP, LOGTMP+1.
04100	EXP:	STAZ	LOGTMP+1;Save sign of input.
04200		ANDI	177	;Mask it off.
04300		BEQ	ZEROIN	;Zero characteristic returns
04400		TAX	;zero.
04500		TYA	;Get the mantissa...
04600		SEC
04700		SBCI	=11	;...subtract 0.043...
04800		STAZ	LOGTMP	;(save this value)
04900		TXA	;...propagate the carry and get rid
05000		SBCI	100	;of the XS-64 offset.
05100		BMI	NEGIN	;If negative (value < 1.0)
05200				;return zero.
05300		CMPI	=15	;Test for overflow (value>=2↑15
05400		BCS	SATUR
05500		TAX	;...no. Number is in range.
05600		ADCI	370	;Is characteristic below 8?
05700		BMI	BLOATE
05800		TAX	;No. Reduce if by 8,
05900		JSR	UNNORM	;unnormalize.
06000		BMI	GETTMP	;Jump.
     

00100	BLOATE:	JSR	UNNORM	;Yes. Unnormalize, then
00200		ASLZ	LOGTMP	;(round result)
00300		ADCI	0
00400		STAZ	LOGTMP	;use result as low byte and
00500		LDAI	0	;set high byte to zero.
00600	
00700	GETTMP:	LDYZ	LOGTMP
00800	GTMP1:	LDXZ	LOGTMP+1;Test sign of input...
00900		BPL	POSIGN
01000		STAZ	LOGTMP+1;...negative. 2's complement
01100		LDAI	0	;the result.
01200		SEC
01300		SBCZ	LOGTMP
01400		TAY
01500		LDAI	0
01600		SBCZ	LOGTMP+1
01700	POSIGN:	RTS
01800	
01900	NEGIN:	LDAI	0	;Set the result to zero if the
02000	ZEROIN:	TAY	;input is negative.
02100		RTS
02200	
02300	SATUR:	LDYI	377	;Saturate result to 2↑15 - 1 if
02400		STYZ	LOGTMP	;input was 15 or more.
02500		LDAI	177
02600		BNE	GTMP1	;Jump.
02700	
02800	UNNORM:	LDAI	1	;Unnormalize subroutine. Add 1
02900		BNE	DECRX	;to the fraction. Jump.
03000	
03100	SCALE:	ASLZ	LOGTMP	;Scale the fraction left by the
03200		ROLA	;amount of the characteristic.
03300	DECRX:	DEX
03400		BPL	SCALE
03500		RTS
03600	
03700	;Enter with characteristic of multiplier in A,
03800	;mantissa in Y, X pointing to a pair of base page
03900	;locations containing the multiplicand (mantissa in the
04000	;low byte).
04100	;Returns the product in A and Y, same form as the
04200	;multiplier. Leaves X unchanged. Clobbers LOGTMP and
04300	;LOGTMP+1.
04400	MULTIP:	PHA
04500		EORZX	1	;Compute sign of result,
04600		STAZ	LOGTMP+1	;save it away.
04700		PLA
04800		ANDI	177	;Mask off multiplier sign.
04900		BEQ	ZEROIN	;If zero, return zero.
05000		STAZ	LOGTMP
05100		TYA	;Add the two logarithms.
05200		CLC
05300		ADCZX	0
05400		TAY
05500		LDAZX	1
05600		ANDI	177	;If multiplicand is zero,
05700		BEQ	ZEROIN	;return a zero.
05800		ADCZ	LOGTMP
05900		SEC
06000		SBCI	100	;Correct the XS-64 offset.
     

00100		BPL	INSIGN	;Result in range?
00200		ANDI	100	;No. If underflow,
00300		BNE	NEGIN	;return zero.
00400		LDAI	177	;Overflow. Saturate to
00500		LDYI	377	;highest magnitude.
00600	
00700	INSIGN:	ASLA	;Insert the sign of the result.
00800		ASLZ	LOGTMP+1
00900		RORA
01000		RTS
01100	
01200	;Inverse function: 2's complement the magnitude part
01300	;of a 15-bit logarithm.
01400	;Enter with characteristic in A, mantissa in Y.
01500	;Returns inverse in the same form. X unchanged.
01600	;Clobbers LOGTMP and LOGTMP+1.
01700	INV:	STYZ	LOGTMP	;Pretty straightforward...
01800		STAZ	LOGTMP+1
01900		SEC
02000		LDAI	0	;Complement the number by
02100		SBCZ	LOGTMP	;subtracting it from zero.
02200		TAY
02300		LDAI	0
02400		SBCZ	LOGTMP+1
02500		JMP	INSIGN	;Insert the original sign.
02600	
02700	;DAC output table.
02800	   LOC (.∨377)+1	;For start of next page.
02900	VETBL:		;DAC output table.
03000	N ← 0
03100	REPEATE 400,{N ↔ N←N+1 ↔}
03200	
03300	   NMI ← START	;Reset??
03400	;Interrupt vectors.
03500	   LOC 177772
03600		NMI∧377
03700		(NMI⊗-10)∧377
03800		START∧377
03900		(START⊗-10)∧377
04000		TICK∧377
04100		(TICK⊗-10)∧377
04200	END